/*************************************************
 *  Object:       Tric! A flying Ricotta.
 *  Authors:      Teo (matteo.piva-gmail.com)
                  Carletto (carlo.checchi-gmail.com)
*************************************************/

#include "config.h"
#include "rx.h"


void RX_processValues(unsigned short *RX_interruptTimer, float *RX_values) {
    unsigned short cnt;
    unsigned short zerocross;
    float RX_conversionFactor;

    RX_conversionFactor = 180.0 / RX_maxTimerValue;

    for(cnt=0; cnt<7; cnt++) {
       if (RX_interruptTimer[cnt] == 255) zerocross = cnt;
    }
    for(cnt=zerocross+1; cnt<7; cnt++) {
        if (RX_conversionFactor * RX_interruptTimer[cnt] > RX_max_value) RX_values[cnt-zerocross-1] = RX_max_value;
        else if (RX_conversionFactor * RX_interruptTimer[cnt] < RX_min_value) RX_values[cnt-zerocross-1] = RX_min_value;
        else RX_values[cnt-zerocross-1] = RX_conversionFactor * RX_interruptTimer[cnt];

    }
    for(cnt=0; cnt<zerocross; cnt++) {
        if (RX_conversionFactor * RX_interruptTimer[cnt] > RX_max_value) RX_values[cnt+6-zerocross] = RX_max_value;
        else if (RX_conversionFactor * RX_interruptTimer[cnt] < RX_min_value) RX_values[cnt+6-zerocross] = RX_min_value;
        else RX_values[cnt+6-zerocross] = RX_conversionFactor * (RX_interruptTimer[cnt] - RX_minTimerValue);

    }

    for(cnt=0; cnt<6; cnt++) {
        switch(cnt) {    // TIP: where 1.41666 is coming from? 255/180 = 1.41666
            case 2:      if (1.41666 * RX_values[cnt] > RX_max_throttle) RX_values[cnt] = RX_max_throttle;
                         else if (1.41666 * RX_values[cnt] < RX_min_throttle) RX_values[cnt] = RX_min_throttle;
                         else RX_values[cnt] = 1.41666 * RX_values[2];
                         break;
            default:     RX_values[cnt] -= 90;
                         break;
        }
    }
}

void RX_applyThrottleToMotors(float *RX_values, unsigned short *MOTORS_speed) {
    unsigned short cnt;

    for(cnt=0; cnt<3; cnt++) {
        if (RX_throttleFactor * RX_values[2] > MOTORS_max_speed ) MOTORS_speed[cnt] = MOTORS_max_speed ;
        else if (RX_throttleFactor * RX_values[2] < MOTORS_min_speed ) MOTORS_speed[cnt] = MOTORS_min_speed ;
        else MOTORS_speed[cnt] += RX_throttleFactor * RX_values[2];
    }
}